%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  临时搭建的用于修补电力塔的脚本。由于之前的分类方法是针对线做的设计，因此对塔只能粗找出上半部分。
%  本脚本利用塔区域的高度连续性进行修补
%
%  脚本流程：
%     1、扫描所有格点中的塔线点，对其按照高度向下搜索。
%     2、每次向下的步长为0.06（可改），当此区间内有新点，且新点的高度在当前格点地面高度之上（参考grid_map.gh定义），则加入
%     3、重复2步骤直到没有新点加入为止。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

disp(['Repairing Tower...']);
for i = 2:2:grid_num-1
    for j = 2:2:grid_num-1
        if (grid_map(i,j).point_num >1)
            if (sum(points(grid_map(i,j).points(:,4),5))>=1)
                temp_points = pick(grid_map,i,j);
                p = sortrows(temp_points,5,'descend');
                %p = sortrows(tp,5,'descend');
                ind =find(p(:,5),1,'last');
                p1 = sortrows(p(1:ind,:),3,'descend');
                p0 = sortrows(p(ind+1:size(p,1),:),3,'descend');
                if size(p0,1) ~=0
                    dh = p1(size(p1,1),3)-p0(1,3);
                    lx = ceil(p0(1,1)*100);
                    ly = ceil(p0(1,2)*100);
                end
                
                % while (size(p0,1)>0 && (dh<0.06))
                while (size(p0,1)>0 && (dh<0.06) && (p0(1,3)>=(grid_map(lx,ly).gh)))
                    newpoint = p0(1,:);
                    
                    points(newpoint(1,4),5) = 1;
                    if size(p0,1)>1
                        p0(1,:) = [];
                        lx = ceil(p0(1,1)*100);
                        ly = ceil(p0(1,2)*100);
                        dh = newpoint(1,3)-p0(1,3);
                        if debug_mode
                            
                            draw(p1,'r');
                            hold on;
                            draw(p0,'b');
                            p1 = [p1;newpoint];
                            draw(newpoint,'g');
                            hold off;
                        end
                        
                    else
                        p0 = [];
                        dh = 999;
                        
                    end
                end
            end
        end
    end
end

function temppoint = pick(grid_map,i,j);
    temppoint = [];
    sd = [-1,0,1];
    for x = 1:3
        for y = 1:3
            temppoint = [temppoint;grid_map(i+sd(x),j+sd(y)).points];
        end
    end
end



